﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace Polar.AI.Common
{
    public static class Calc
    {
        #region Usefull Constants...
        public const float Radian180 = 3.1415f;
        public const float Radian90 = 1.5707f;
        public const float Radian45 = 0.785f;
        public const float Cos45 = 0.707f;
        #endregion

        #region Basic Calculations...
        /// <summary>
        /// This function is only used to make calculations more readable
        /// </summary>
        public static float Square(float x)
        {
            return x * x;
        }

        public static bool FloatEqualTo(float value, float compare)
        {
            return value > compare - 0.1f && value < compare + 0.1f; 
        }

        #endregion

        #region Vector Calculations...
        /// <summary>
        /// Give the radian angle between two normalized vectors
        /// </summary>
        public static float VectorAngleRadians(Vector2 unitVector1, Vector2 unitVector2)
        {
            return (float)(Math.Acos(Vector2.Dot(unitVector1, unitVector2)));
        }

        /// <summary>
        /// Get a perpendicular vector from two vectors???
        /// </summary>
        public static Vector2 Perpendicular(Vector2 a, Vector2 b)
        {
            return new Vector2((b.Y - a.Y) * -1, b.X - a.X);
        }

        /// <summary>
        /// Get component of vector parallel to a unit basis vector.
        /// </summary>
        public static Vector2 ParallelComponent(Vector2 vector, Vector2 unitBasis)
        {
            float projection = Vector2.Dot(vector, unitBasis);
            return unitBasis * projection;
        }

        /// <summary>
        /// Get component of vector perpendicular to a unit basis vector.
        /// </summary>
        public static Vector2 PerpendicularComponent(Vector2 vector, Vector2 unitBasis)
        {
            return (vector - ParallelComponent(vector, unitBasis));
        }

        /// <summary>
        /// Returns the scalar projection of body's forward direction over the distance between the target and body.
        /// </summary>
        public static float ScalarProjection(Vector2 targetPosition, Vector2 bodyPosition, Vector2 unitForward)
        {
            return Vector2.Dot(targetPosition - bodyPosition, unitForward);
        }

        /// <summary>
        /// Calculates the point of impact of two bodies heading towards each other base on relative velocity.
        /// </summary>
        /// <param name="sourcePosition">Position of source.</param>
        /// <param name="sourceLinearVelocity">LinearVelocity of source.</param>
        /// <param name="targetPosition">Position of target.</param>
        /// <param name="targetLinearVelocity">LinearVelocity of target.</param>
        /// <returns>Returns the final position of source body upon impart.</returns>
        public static Vector2 HeadOnLinearVelocityCollisionPoint(Vector2 sourcePosition, Vector2 sourceLinearVelocity, Vector2 targetPosition, Vector2 targetLinearVelocity)
        {
            var velocitySum = new Vector2(Math.Abs(sourceLinearVelocity.X) + Math.Abs(targetLinearVelocity.X),
                Math.Abs(sourceLinearVelocity.Y) + Math.Abs(targetLinearVelocity.Y));

            var x = sourceLinearVelocity.X == 0.0f ? 0.0f : sourceLinearVelocity.X * (Math.Abs(sourceLinearVelocity.X) / velocitySum.X);
            var y = sourceLinearVelocity.Y == 0.0f ? 0.0f : sourceLinearVelocity.Y * (Math.Abs(sourceLinearVelocity.Y) / velocitySum.Y);

            return sourcePosition + new Vector2(x, y);
        }
        #endregion

        #region Position/Velocity Calculations...
        /// <summary>
        /// Given a distance to travel predicts a body's position
        /// </summary>
        public static Vector2 PredictPositionFromDistance(IBody source, float distance)
        {
            if (source.LinearVelocity.IsZero())
                return source.Position;
            else
                return source.Position + Vector2.Normalize(source.LinearVelocity) * distance;
        }

        /// <summary>
        /// Determins if a velocity is over a specific speed
        /// </summary>
        public static bool IsOverMaxSpeed(Vector2 velocity, float maxSpeed)
        {
            float maxLengthSquared = maxSpeed * maxSpeed;
            float vecLengthSquared = velocity.LengthSquared();
            return vecLengthSquared > maxLengthSquared;
        }
        #endregion

        #region Angle Calculations...
        /// <summary>
        /// Applies the trig function x / sin(r1) * sin(r2)
        /// </summary>
        public static float TrigSineRule(float side1, float radian1, float radian2)
        {
            return (float)(side1 / (float)Math.Sin(radian1) * (float)Math.Sin(radian2));
        }


        /// <summary>
        /// Uses the trig trick of slicing triangle into two right angles to determine passing distance.
        /// </summary>
        public static float TrigPassingDistance(float separationDistance, float bodyAngleToTarget)
        {
            throw  new Exception("Not working");
            //if (bodyAngleToTarget > Calc.Radian90)
            //    throw new NotSupportedException("BodyAngltToTarget must be < Radian90 to calculate the Trig Passing Distance.");

            //float angleAt90 = Calc.Radian180 - (Calc.Radian90 + bodyAngleToTarget);

            

            
            //float separationAngle = angleAt90 + bodyAngleToTarget;
            
            //float passingDistance = TrigSineRule(separationDistance, separationAngle, bodyAngleToTarget);

            //return passingDistance;
        }

        /// <summary>
        /// Returns the angle expressed in radians between -Pi and Pi.
        /// </summary>
        public static float WrapAngle(float radians)
        {
            while (radians < -MathHelper.Pi)
            {
                radians += MathHelper.TwoPi;
            }
            while (radians > MathHelper.Pi)
            {
                radians -= MathHelper.TwoPi;
            }
            return radians;
        } 

        #endregion

        #region Private Static Methods...
        private static float SimulateOvertakeDistance(float srv, float trv, float distance)
        {
            if (srv == trv) return distance;  //they are going same speed so travel same distance

            if (srv != 0 && distance != 0)
            {
                if (srv < trv)
                    throw new NotSupportedException("Source relative velocity must be larger then target relative velocity in SimulateOvertakeDistance.");

                float closeDistance = distance;
                float sumDistance = distance;

                int index = 0;
                //TODO : Hacked this together, find a proper calculation for determining overtake distance
                while (Math.Abs(closeDistance) > 0.05f)
                {
                    if (index++ > 100) throw new DataMisalignedException(string.Format("Infinite loop detected with data: s:{0} t:{1} d:{2}", srv, trv, distance));

                    closeDistance = (closeDistance / srv) * trv;
                    sumDistance += closeDistance;
                }
                return sumDistance;
            }
            else
            {
                return 0;
            }
        }

        #endregion
    }
}
